#include "headfile.h"
#include "icm.h"

float icm_acc_angle = 0;
float icm_gyro_angle = 0;
float icm_out_angle = 0;
float icm_gyro_yf = 0;
float icm_k=0.05;
//float gyro_yf_zero = -0.22137;

void icm_init(void){
    int i;
	icm20602_init_spi();
	delay_ms(60);		//延时是为了等待ICM初始化
    icm_gyro_angle = 0;
    for(i=0;i<10;i++){
        get_icm20602_accdata_spi();
	    icm_gyro_angle += -atan2( (double)icm_acc_x, (double)icm_acc_z) * 180.0 / PI;
    } 
    icm_gyro_angle /=10;
}

void icm_set_k(float c){
    icm_k=c;
}

void icm_angle_calculate(void){
    icm_acc_angle = - atan2((double)icm_acc_x, (double)icm_acc_z) * 180 / PI;
	icm_gyro_yf = (float)icm_gyro_y * 2000.0 / 32768.0 ;//- gyro_yf_zero;		 //将陀螺仪的角速度单位转换为°/s
    icm_gyro_angle = icm_out_angle + icm_gyro_yf * 0.005;
    icm_out_angle = icm_gyro_angle*(1 - icm_k) + icm_k * icm_acc_angle;
}

void icm_exe(void){
    get_icm20602_gyro_spi();
    get_icm20602_accdata_spi();
    icm_angle_calculate();
}
